#include "pid.h"

// 速度环，全工程维护
pid_t pid_speed;
// 方向环，全工程维护
pid_t pid_dir;

/**
 * @brief PID初始化
 *
 * @param pid 闭环结构体
 * @param kp 比例
 * @param ki 积分
 * @param kd 微分
 * @param output_max 最大限幅
 * @param output_min 最小限幅
 */
void pid_init(pid_t *pid, float kp, float ki, float kd, float output_max, float output_min)
{
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;

    pid->err = 0;
    pid->last_err = 0;
    pid->prev_err = 0;

    pid->output = 0;
    pid->output_max = output_max;
    pid->output_min = output_min;
}

/**
 * @brief 增量式PID计算
 * @note 可能这个用不上，因为误差计算我在另外的任务中实现了，与此函数解耦
 * @param pid 闭环结构体
 * @param target 目标值
 * @param measure 测量值
 * @return float 输出值
 */
float pid_calculate(pid_t *pid, float target, float measure)
{
    pid->err = target - measure;

    // 计算PID增量
    float delta_output = pid->kp * (pid->err - pid->last_err) + pid->ki * pid->err + pid->kd * (pid->err - 2 * pid->last_err + pid->prev_err);

    // 更新历史误差
    pid->prev_err = pid->last_err;
    pid->last_err = pid->err;

    // 累加增量到总输出
    pid->output += delta_output;

    // 限幅
    if (pid->output > pid->output_max)
    {
        pid->output = pid->output_max;
    }

    else if (pid->output < pid->output_min)
    {
        pid->output = pid->output_min;
    }

    // 返回总输出
    return pid->output;
}


